import cv2
from PyQt5.Qt import QThread
from PyQt5 import QtCore
from PyQt5.QtGui import QPixmap, QImage
from PyQt5.QtCore import Qt, QPoint, QRect, pyqtSignal
# from cnocr import CnOcr
import modbus_tk.defines as cst
from modbus_tk import modbus_tcp
import time, logging, math, imutils, json, socket, threading
import numpy as np
import struct
import os

import base64
import platform


logger = logging.getLogger(__name__)
logger.setLevel(logging.DEBUG)

kernel_3 = np.ones((3, 3), np.uint8)  # 3x3的卷积核
kernel_5 = np.ones((5, 5), np.uint8)  # 4x4的卷积核
kernel_7 = np.ones((7, 7), np.uint8)

draw_color = (0,255,0)

Lower_red = np.array([66, 0, 130])    #red
Upper_red = np.array([168, 255, 255])
draw_red = (0,0,0)
red = [Lower_red, Upper_red, 'red',draw_red]

Lower_blue = np.array([113, 97, 95])
Upper_blue = np.array([120, 189, 225])
draw_blue =draw_color
blue = [Lower_blue, Upper_blue, 'blue',draw_blue]

Lower_green = np.array([39, 60, 133])
Upper_green = np.array([96, 255, 255])
draw_green =draw_color
green = [Lower_green, Upper_green, 'green',draw_green]

Lower_yellow = np.array([21, 103, 63])   
Upper_yellow = np.array([37, 255, 255])
draw_yellow = draw_color
yellow = [Lower_yellow, Upper_yellow, 'yellow',draw_yellow]

#### 形状识别七巧板颜色HSV值
Lower_black = np.array([70, 100, 100])
Upper_black = np.array([87, 150, 120])
draw_black = (0,0,0)
black = [Lower_black, Upper_black, 'black',draw_black]

color_list = [red,blue,green,yellow]

h =25
w = 25
max_area = 40000
min_area = 3000
camera_nun = 0


#### x,y 检测时皮带机上托盘仓位的像素坐标,左上角（x_1,y_1）右下角（x_2,y_2）
x_1 = 242
x_2 = 378
y_1 = 178
y_2 = 298

current_dir = os.path.dirname(os.path.abspath(__file__))

class Camera_Thread(QThread):

    cap_signal = pyqtSignal(str)

    def __init__(self,label, gui_signal):
        super().__init__()
        self.stop = False
        self.Is_exit = False
        self.cap= None
        self.frame = None
        self.program_flag = 10 # 标记程序运行索引
        self.frame_counter = 0
        self.ocr_interval = 5

        self.label = label
        self.selected_area = None 

        self.gui_signal=gui_signal
        self.cap_signal.connect(self.cap_camera)

        ### 所需文件夹路径确认
        self.pathCheck()
        self.virtualCam = IOACamera()

        self.count_pos = 0
        # self.cam_TCP()

        self.master = Manger_dis(self.gui_signal)
        self.master.start()
        
        self.position_init = {"1":[],"2":[],"3":[],"4":[],
                           "A":[],"B":[],"C":[],"D":[],
                           "red":[],"blue":[],"green":[],"yellow":[],
                           "circle":[],"triangle":[],"rectangle":[],"hexagon":[],
                           }
        self.position = self.position_init

        self.postion_Pose = None

    def pathCheck(self):
        # 获取当前脚本所在目录
        

        # 定义需要检查/创建的文件夹路径（Windows格式）
        paths = [
            os.path.join(current_dir, "vision", "image"),       # 当前目录下的相对路径
            os.path.join(current_dir, "vision", "result"),
            os.path.join(current_dir, "vision", "img"),
            # 如果要用绝对路径（示例）：
            # r"C:\cat\gm\image\camera",
            # r"C:\cat\gm\image\algo",
            # r"C:\cat\gm\algorithm"
        ]

        for path in paths:
            # 创建文件夹（如果不存在）
            os.makedirs(path, exist_ok=True)
            print(f"目录已确认：{path}")

        print("所有文件夹检查完毕！")

    def cam_TCP(self):
        self.cam_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.cam_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, True)
        self.cam_socket.bind(('0.0.0.0', 8888))
        self.cam_socket.listen(10)

    def exit(self):
        self.stop = True
        self.Is_exit= True
        self.cap.release()
    
    #### Application OCR--二维码--形状--垃圾分拣（颜色）
    
    def ocr_test(self,img):
        try:
            ### 所有参数都使用默认值即可，查看可直接进入函数
            ocr = CnOcr()  
            ress = ocr.ocr(img)
                
            if ress:
                for res in ress:
                    text = res['text']  
                    position = res['position']   
                    ### 计算矩形中心  
                    x1, y1 = int(position[0][0]), int(position[0][1])  
                    x2, y2 = int(position[2][0]), int(position[2][1])  
                    center_x = int((x1 + x2) / 2)  
                    center_y = int((y1 + y2) / 2)
                    feature = [center_x,center_y,0] 
                    
                    ### 检测目标绘制矩形框 
                    cv2.rectangle(self.frame, (x1, y1), (x2, y2), (0, 255, 0), 2)
                    ### 检测目标中心画圆
                    cv2.circle(self.frame, (center_x, center_y), 5, (0, 0, 255), -1)

                    if text == "A" or text== "4" or text == "^" or text == "Y":
                        self.position["A"] = feature
                    elif text == "B" or text == "8":
                        self.position["B"] = feature
                    elif text == "C":
                        self.position["C"] = feature
                    elif text == "D" or text == "0" or text == "O" or text == "o":
                        self.position["D"] = feature
                    print("识别到的字符为：%s" %text)
                    ### 图形显示识别到的字符

                    # match text:
                    #     case "A" | "4" | "^" | "Y":
                    #         self.position["A"] = feature
                    #         self.master.action_flag = 5
                    #     case "B" | "8":
                    #         self.position["B"] = feature
                    #         self.master.action_flag = 6
                    #     case "C":
                    #         self.position["C"] = feature
                    #         self.master.action_flag = 7
                    #     case "D" | "0" | "O" | "o":
                    #         self.position["D"] = feature
                    #         self.master.action_flag = 8                         
                    print("识别到的字符为：%s" %text)
                    ### 图形显示识别到的字符
                    cv2.putText(self.frame, text, (int(x1), int(y1)- 10), cv2.FONT_HERSHEY_SIMPLEX, 2, (0, 0, 255), 2)
                    # cv2.imwrite("img/output.png",self.frame)
                    # cv2.imwrite("result/OCR_result.png",self.frame)

        except Exception as e:
            logger.error(e,exc_info=True)

    def QR_test(self, img):
        try:  
            detect_obj = cv2.wechat_qrcode_WeChatQRCode('detect.prototxt', 'detect.caffemodel', 'sr.prototxt', 'sr.caffemodel')  
            res, points = detect_obj.detectAndDecode(img)  
    
            if points is not None:  
                for idx, qr_points in enumerate(points):  
                    # 计算中心点坐标（取四个角点坐标的平均值）  
                    x0, y0 = qr_points[0]  
                    x1, y1 = qr_points[2]  # 假设 qr_points 是按顺时针或逆时针顺序存储的四个角点  
                    center_x = (x0 + x1) / 2  
                    center_y = (y0 + y1) / 2  
                    center_point = (int(center_x), int(center_y))  
                    feature = [int(center_x), int(center_y), 0]
                    self.QR_value = int(res[idx])

                    if abs(x_2 - center_x) < 30 and  abs( y_1 - feature[1]) < 30:
                        for i in range(1,5):
                            if i == self.QR_value:
                                self.master.master.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=260, output_value=1)
                                time.sleep(0.6)
                                value_tmp = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=260, quantity_of_x=1)[0]
                                while value_tmp == 1:
                                    time.sleep(0.5)
                                    value_tmp = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=260, quantity_of_x=1)[0]
                                
                                self.master.master.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=261, output_value=i)
                                time.sleep(0.5)
                                value_tmp2 = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=261, quantity_of_x=1)[0]
                                
                                while value_tmp2 == i:
                                    time.sleep(0.5)
                                    value_tmp2 = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=261, quantity_of_x=1)[0]            
                    
                    elif abs(x_2 - center_x) < 30 and  abs( y_2 - feature[1]) < 30:
                        for i in range(1,5):
                            if i == self.QR_value:
                                self.master.master.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=260, output_value=2)
                                time.sleep(0.6)
                                value_tmp = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=260, quantity_of_x=1)[0]
                                while value_tmp == 2:
                                    time.sleep(0.5)
                                    value_tmp = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=260, quantity_of_x=1)[0]
                                
                                self.master.master.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=261, output_value=i)
                                time.sleep(0.5)
                                value_tmp2 = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=261, quantity_of_x=1)[0]
                                
                                while value_tmp2 == i:
                                    time.sleep(0.5)
                                    value_tmp2 = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=261, quantity_of_x=1)[0]    
                    
                    elif abs(x_1 - center_x) < 30 and  abs( y_1 - feature[1]) < 30:
                        for i in range(1,5):
                            if i == self.QR_value:
                                self.master.master.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=260, output_value=3)
                                time.sleep(0.6)
                                value_tmp = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=260, quantity_of_x=1)[0]
                                while value_tmp == 3:
                                    time.sleep(0.5)
                                    value_tmp = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=260, quantity_of_x=1)[0]
                                
                                self.master.master.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=261, output_value=i)
                                time.sleep(0.5)
                                value_tmp2 = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=261, quantity_of_x=1)[0]
                                
                                while value_tmp2 == i:
                                    time.sleep(0.5)
                                    value_tmp2 = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=261, quantity_of_x=1)[0]    
                    
                    elif abs(x_1 - center_x) < 30 and  abs( y_2 - feature[1]) < 30:
                        for i in range(1,5):
                            if i == self.QR_value:
                                self.master.master.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=260, output_value=4)
                                time.sleep(0.6)
                                value_tmp = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=260, quantity_of_x=1)[0]
                                while value_tmp == 4:
                                    time.sleep(0.5)
                                    value_tmp = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=260, quantity_of_x=1)[0]
                                
                                self.master.master.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=261, output_value=i)
                                time.sleep(0.5)
                                value_tmp2 = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=261, quantity_of_x=1)[0]
                                
                                while value_tmp2 == i:
                                    time.sleep(0.5)
                                    value_tmp2 = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=261, quantity_of_x=1)[0]   

                    # 在图像上绘制二维码的中心点（绘制一个绿色的圆来表示中心点）  
                    cv2.circle(self.frame, center_point, 5, (0, 255, 0), -1)  
    
                    # 绘制二维码的边界线  
                    color = (0, 0, 255)  
                    thick = 3  
                    for p in [(0, 1), (1, 2), (2, 3), (3, 0)]:  
                        start = (int(qr_points[p[0]][0]), int(qr_points[p[0]][1]))  
                        end = (int(qr_points[p[1]][0]), int(qr_points[p[1]][1]))  
                        cv2.line(self.frame, start, end, color, thick)  
    
                    # 处理二维码解码结果  
                    qr_code_text = res[idx]
                    print(f"qr_code{qr_code_text[:1]}, feature is {feature}")
                    if qr_code_text == "1":
                        self.position["1"] = feature
                    elif qr_code_text == "2":
                        self.position["2"] = feature
                    elif qr_code_text == "3":
                        self.position["3"] = feature
                    elif qr_code_text == "4":
                        self.position["4"] = feature
                    else:
                        pass

                    # 绘出图像上二维码的数据（这里假设只显示第一个字符作为示例）  
                    # 注意：如果 res[idx] 不是字符串或者格式不符合预期，这里可能会出错  
                    cv2.putText(self.frame, qr_code_text[:1], (int(x0), int(y0) - 10), cv2.FONT_HERSHEY_SIMPLEX, 2, (0, 0, 125), 2)  
    
                self.master.master.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=261, output_value=0)
                self.master.master.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=260, output_value=0)
                self.master.master.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=181,output_value=0) 
                # 保存处理后的图像  
                cv2.imwrite("img/output.png", self.frame)  
                cv2.imwrite("vision/result/QR_result.png", self.frame)  
            return self.postion_Pose
    
        except Exception as e:
            logger.error(e, exc_info=True)

    def waining_angle(self,):
        try:
            pass
        except Exception as e:
            logger.error(e,exc_info=True)

    def triangle_angle(self,approx):
        try:
            approx = approx.tolist()
            centroid = [(approx[0][0][0] + approx[1][0][0] + approx[2][0][0]) / 3,
                        (approx[0][0][1] + approx[1][0][1] + approx[2][0][1]) / 3]
            distances = [math.sqrt(math.pow(vertex[0][0] - centroid[0], 2) + math.pow(vertex[0][1] - centroid[1], 2)) for vertex in approx]
            max_distance = max(distances)
            max_index = distances.index(max_distance)
            angle_rad = math.atan2(approx[max_index][0][1] - centroid[1], approx[max_index][0][0] - centroid[0])
            angle_deg = math.degrees(angle_rad)
            self.master.master.execute(1, cst.WRITE_SINGLE_REGISTER, 9, output_value=int(angle_deg))
            # quo, rema = divmod(angle_deg, 60)
            rema = angle_deg % 120
            #### add determinate
            if rema  < 60:
                rema = rema
            elif rema > 60 and rema < 120:
                rema = rema -120
            return int(rema)

        except Exception as e:
            print("Error:", e)
            return None

    def hexagon_angle(self,approx):
        try:
            approx = approx.tolist()

            # 计算中心点
            centroid = [(approx[0][0][0] + approx[1][0][0] + approx[2][0][0] + approx[3][0][0] + approx[4][0][0] + approx[5][0][0]) / 6,
                        (approx[0][0][1] + approx[1][0][1] + approx[2][0][1] + approx[3][0][1] + approx[4][0][1] + approx[5][0][1]) / 6]

            # 计算顶点到中心的距离
            distances = [math.sqrt(math.pow(vertex[0][0] - centroid[0], 2) + math.pow(vertex[0][1] - centroid[1], 2)) for vertex in approx]

            # 找到距离最远的顶点
            max_distance = max(distances)
            max_index = distances.index(max_distance)

            # 计算旋转角度
            angle_rad = math.atan2(approx[max_index][0][1] - centroid[1], approx[max_index][0][0] - centroid[0])
            angle_deg = math.degrees(angle_rad)

            rema = angle_deg % 60
            if rema < 30:
                rema = rema
            elif  rema > 30 and rema < 60:
                rema = rema - 60
            return int(rema)

        except Exception as e:
            print(f"Error: {e}")
            return None

    # 计算角度
    def angel_count(self,cnt_max):
        try:
            peri = cv2.arcLength(cnt_max,True)
            # 获取轮廓顶点
            approx = cv2.approxPolyDP(cnt_max,0.04*peri,True)   
            point_num =  len(approx)
            

            # 最小的外接矩形
            rect = cv2.minAreaRect(cnt_max)
            # 获取最小外接矩形的4个顶点
            box = cv2.boxPoints(rect)  
            box = np.int0(box)
            theta = 0
            if 0 not in box.ravel():
                # 旋转角度
                theta = cv2.minAreaRect(cnt_max)[2]
                # 三角形的单独计算角度
                # if point_num == 3:     
                #     theta = self.triangle_angle(approx)
                # # 其余角度全部按外接四边形方法
                # else:

                
                # 获取宽度和高度，第一个是宽度   
                width, height = cv2.minAreaRect(cnt_max)[1]   
                if width < height :
                    theta = theta+90
                    if int(-(180 - theta)) < -69:
                        theta = theta + 90
                # #### 逆时针方向
                elif width > height:
                    theta = 180 + theta
                
                else: 
                    theta = theta
                # print('图片的旋转角度为%s.'%theta)
            return int(-(180- theta))
            # return theta

        except Exception as e:
            logger.error(e,exc_info=True)

    # 根据轮廓画出框框
    def draw_rect(self,radium,cnt_max):
        # 画圆的圆心、半径
        x, y, r = int(radium[0]), int(radium[1]), int(radium[2])
        cv2.circle(self.frame, (x, y), r, (0, 0, 0), 3)

    # 测试出最大的轮廓
    def max_contour(self,radium,contours,name):
        are_max = 30   # 定义一个初始的轮廓最大面积
        cnt_max = None   # 存储一个最大的轮廓
        flag = 0
        feature = []
        for cnt in contours:
            self.area = cv2.contourArea(cnt)
            # 最小外界矩形的宽度和高度
            if self.area > are_max:
                flag = 1
                are_max = self.area
                cnt_max = cnt
        if flag:

            angle = self.angel_count(cnt_max)
            cX = int(radium[0])
            cY = int(radium[1])
            if name == "red":
                name = "Harmful"
            elif name == "green":
                name = "Kitchen"
            elif name == "blue":
                name = "Recycle"
            elif name == "yellow":
                name = "Other"
            # match name:
            #     case "red":
            #         name = "Harmful"
            #     case "green":
            #         name = "Kitchen"
            #     case "blue":
            #         name = "Recycle"
            #     case "yellow":
            #         name = "Other"

            # self.frame = cv2.drawContours(self.frame, [cnt_max], -1,  (0, 0, 255), 3)  # 画出轮廓
            self.draw_rect(radium,cnt_max=cnt_max)
            cv2.circle(self.frame, (cX , cY ), int(radium[2]), (0, 0, 0), 1)
            cv2.putText(self.frame, name, (cX - 20, cY - 20),cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 2)  
            
            feature.extend([cX,cY])
            feature.append(angle)
            
        return feature

    #### 形状识别
    def contour_recong(self,frame):
        # # 进行高斯模糊操作
        # blurred = cv2.GaussianBlur(frame, (5, 5), 0)
        # # 进行图片灰度化
        # gray = cv2.cvtColor(blurred, cv2.COLOR_BGR2GRAY)
        # # 进行阈值分割
        # thresh = cv2.threshold(gray, 60, 255, cv2.THRESH_BINARY)[1]
        # # 在二值图片中寻找轮廓
        # contours = cv2.findContours(thresh.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        # contours = imutils.grab_contours(contours)
        x_0, y_0, w_0, h_0 = 110, 10, 500, 415 
    
        # 裁剪图片  
        ori = frame[y_0 : y_0 + h_0, x_0 : x_0 + w_0]
        # cv2.imwrite('result/roi.png', ori)
  
        HSV = cv2.cvtColor(ori, cv2.COLOR_BGR2HSV)
        color = red
        mask_= cv2.inRange(HSV, color[0], color[1])
        # cv2.imwrite('mask.jpg', mask_)
        ## 腐蚀  膨胀
        erosion = cv2.erode(mask_, kernel_3, iterations=1)   
        dilation = cv2.dilate(erosion, kernel_5, iterations=1) 

        # 二值化，将滤波后的图像变成二值图像放在binary中
        ret, binary = cv2.threshold(dilation, 0, 255, cv2.THRESH_BINARY)
        # cv2.imwrite('./result/binary.jpg', binary)

        inverted_binary = cv2.bitwise_not(binary)
        # cv2.imwrite('./result/inverted_binary.jpg', inverted_binary)
        cnts = cv2.findContours(inverted_binary.copy(), cv2.RETR_EXTERNAL,
                                cv2.CHAIN_APPROX_SIMPLE)
        contours = imutils.grab_contours(cnts)

        # output_frame = frame.copy()  
        # cv2.drawContours(output_frame, contours, -1, (0, 255, 0), 2)
        # cv2.imwrite('./result/contours.png', output_frame)  

        if contours is not None:
            for cnt in contours:  
                # 计算轮廓的边界框  
                x_, y_, w, h = cv2.boundingRect(cnt)
                x = x_ + x_0
                y = y_ + y_0

                contour_area = cv2.contourArea(cnt)
                if contour_area > min_area and contour_area < max_area:
                    cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 3)
                    approx = cv2.approxPolyDP(cnt, 0.016 * cv2.arcLength(cnt, True), True)
                    n_corners = len(approx)
                    # print(f"n_corners {n_corners}")
                    
                    if n_corners == 3:
                        angle = self.triangle_angle(approx)
                        M = cv2.moments(cnt)
                        center_tri = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
                        # cv2.drawContours(self.frame, [approx], -1, (0, 255, 0), 2)
                        self.position["triangle"] = [center_tri[0]+ x_0, center_tri[1]+y_0, angle]
                        print(f" triangle n_corners is {n_corners}")
                        # self.master.action_flag = 22

                    elif n_corners == 4: 
                        angle = self.angel_count(cnt)
                        if abs(cv2.contourArea(cnt) / ((w*h) ** 0.5)) > 0.95:    
                            M = cv2.moments(cnt)
                            center_square = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
                            # cv2.drawContours(self.frame, [approx], -1, (0, 255, 0), 2)
                            self.position["rectangle"] = [center_square[0]+ x_0, center_square[1]+y_0, angle]
                            print(f"rectangle n_corners is {n_corners}")
                            # self.master.action_flag = 24
                    
                    elif n_corners > 4 and n_corners < 8: 
                        angle = self.hexagon_angle(approx)
                        M = cv2.moments(cnt)
                        center_hexagon = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
                        # cv2.drawContours(self.frame, [approx], -1, (0, 255, 0), 2)
                        self.position["hexagon"] = [center_hexagon[0]+ x_0, center_hexagon[1]+y_0, angle]
                        print(f"heaxgon n_corners is {n_corners}")
                        # self.master.action_flag = 23

                    elif n_corners > 7 and n_corners < 100: 
                        angle = self.angel_count(cnt)  
                        M = cv2.moments(cnt)
                        center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
                        # cv2.drawContours(self.frame, [approx], -1, (0, 255, 0), 2)
                        self.position["circle"] = [center[0]+ x_0, center[1]+y_0, 0]
                        pos_1 = self.position["circle"]
                        print(f"circle  n_corners is {n_corners}, postiontion is {pos_1}")
                        # self.master.action_flag = 21
                    
                    cv2.imwrite("/home/marvsmart/factory/vision/result/Shape_result.png",self.frame)
                    cv2.imwrite("/home/marvsmart/factory/vision/img/output.png",self.frame)

    # 颜色识别
    def color_recong(self,frame):
        #### 转换为灰度图像
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        #### 应用高斯模糊
        blurred = cv2.GaussianBlur(gray, (5, 5), 0)
        #### 检测圆形
        circles = cv2.HoughCircles(blurred, cv2.HOUGH_GRADIENT, 1.2, 100)
        if circles is not None:
            for circle_ in circles[0]:
                mask1 = np.zeros_like(frame)
                x, y, r = int(circle_[0]), int(circle_[1]), int(circle_[2])
                inner_radius = r + 10
                #### 绘制大圆
                cv2.circle(mask1, (x, y), inner_radius, (255, 255, 255), -1)
                outer_circle = cv2.bitwise_and(frame, mask1)
                mask1 = np.zeros_like(frame)
                #### 绘制小圆        
                cv2.circle(mask1, (x, y), r, (255, 255, 255), -1)
                inner_circle_inverse = cv2.bitwise_not(mask1)
                annulus_area = cv2.bitwise_and(outer_circle, inner_circle_inverse)
                
                ##### 把BGR图像转换为HSV格式
                HSV = cv2.cvtColor(annulus_area, cv2.COLOR_BGR2HSV)  
                for color in color_list:
                    mask_red = cv2.inRange(HSV, color[0], color[1])
                    #### 腐蚀 膨胀
                    erosion = cv2.erode(mask_red, kernel_3, iterations=1)   
                    dilation = cv2.dilate(erosion, kernel_5, iterations=1)  
                    #### 二值化，将滤波后的图像变成二值图像放在binary中
                    ret, binary = cv2.threshold(dilation, 0, 255, cv2.THRESH_BINARY)  
                    cnts = cv2.findContours(binary.copy(), cv2.RETR_EXTERNAL,
                                            cv2.CHAIN_APPROX_NONE)
                    cnts = imutils.grab_contours(cnts) 
                    self.max_contour(circle_,cnts,color[2])
                    #### 更新位置
                    self.position[color[2]] = [int(circle_[0]), int(circle_[1]), 0]       

    #### 展示图片
    def display(self,):
        try:
            frame = cv2.imread("vision/img/output.png")  
            if frame is None:  
                # 如果图像加载失败，记录错误并退出函数  
                logger.error("Failed to load image 'vision/img/output.png'")  
                return  
  
            # 调整图像大小  
            img = cv2.resize(frame, (1200, 600))  
  
            # 转换颜色空间  
            rgb_img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)  
  
            # 将图像转换为QImage  
            h, w, ch = rgb_img.shape  
            bytes_per_line = ch * w  
            img_dis = QImage(rgb_img.data, w, h, bytes_per_line, QImage.Format_RGB888)  
            img_dis = QPixmap.fromImage(img_dis)  
  
            # 设置label的图像和对其方式  
            self.label.setAlignment(Qt.AlignCenter)  
            self.label.setPixmap(img_dis)  
  
            # 睡眠一小段时间，通常用于调试或控制更新频率  
            time.sleep(0.001)  
        except Exception as e:
            logger.error(e,exc_info=True)

    def save_img(self,name="frame.png"):
        img = self.frame.copy()
        path = os.path.join(src_img , name)
        cv2.imwrite(path,img)

    def run(self): 

        while not self.Is_exit:
            while not self.stop:
                try:
                    self.program_flag = self.master.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=181,quantity_of_x=1)[0]
                    
                    self.frame = self.virtualCam.get_image()
                    # time.sleep(0.1)
                    # self.virtualCam.show()

                    # 在窗口中显示处理之后的画面
                    if self.frame is not None:
                        self.position  = self.position_init

                        image = self.frame.copy()

                        if self.program_flag ==0:
                            pass

                        #### 进行颜色检测识别--垃圾分拣
                        elif self.program_flag ==1:
                            self.color_recong(image)
                        
                        #### 进行轮廓检测识别
                        elif self.program_flag == 2:
                            self.contour_recong(image)  
                        
                        #### 进行二维码识别
                        elif self.program_flag == 3:
                            self.QR_test(image)
                            # self.master.master.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=181,output_value=0) 
                        
                        #### 进行OCR检测识别
                        elif self.program_flag == 4:
                            self.ocr_test(image)  
                        
                        self.master.program_flag = self.program_flag
                        self.master.position = self.position
                        cv2.imwrite( "vision/img/output.png", self.frame) 
                        time.sleep(0.1)
                        self.display()

                except Exception as e:
                    logger.error(e,exc_info=True)
            
    def cap_camera(self,msg):
            try:
                
                message_json = json.loads(msg)
                source = int(message_json.get("source"))
                print(f"camera_thread msg is {msg}, source is {source}")

                
            except Exception as e:
                logger.error(e,exc_info=True)




class Manger_dis(QThread):


    def __init__(self,signal) :
        super().__init__()
        self.run_flag = 1
        self.gui_signal =signal
        self.action_flag = 0       # 定义动作
        self.auto_flag = 0         # 自动运行标志位
        self.position = None       # 

        self.camera_position = 0   #相机拍照位置
        self.home_position = 0     #机器人初始位置
        self.program_flag = 10       # 标记运行的程序
        self.auto_first = 0

        # 定义机器人动作之前的信号
        self.img_ready = 0
        self.robot_stop = 0
        self.robot_start = 0
        self.robot_ready = 0
        self.calibrate_4Points()
        
        # self.robot = robot_.Robot_Driver()
        self.master_init()

    def master_init(self,):
        #### host主机名为本地IP
        self.host = socket.gethostname()
        self.master = modbus_tcp.TcpMaster(host= self.host)
        self.master.set_timeout(5.0)

    def int2float(self,a,b):
        value=0
        try:
            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z=z0+z1 #高字节在前 低字节在后
            value=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
            if value is None:
                return value
            if isinstance(value, float):
                # 精确到小数点后两位数
                return round(value , 2)
            else:
                return value
        except BaseException as e:
            logger.error(e,exc_info=True)
        return value

    def float_to_int16s(self,f):
        # 将浮点数打包成32位二进制数据
        b = struct.pack('f', f)
        # 将32位二进制数据拆分成两个16位二进制数据
        i1, i2 = struct.unpack('HH', b)
        return i2, i1

    ##### 此函数专门向服务器发送指令

    def tcp_send(self, value, cmd):
        try:
            ### 机器人坐标存储到寄存器中
            coordinate_ = []
            feature_ = []
            for i in range(len(value)):
                a = self.float_to_int16s(value[i])
                coordinate_.append(a[0])
                coordinate_.append(a[1])
            
            b = self.float_to_int16s(cmd)
            feature_.append(b[0])
            feature_.append(b[1])

            # print(f"coordinate_:{coordinate_} feature_{feature_}")
            coordinate_xy = coordinate_[0:4]
            coordinate_roll = coordinate_[4:6]
            self.master.execute(1,cst.WRITE_MULTIPLE_REGISTERS,starting_address=117,output_value=coordinate_xy)
            self.master.execute(1,cst.WRITE_MULTIPLE_REGISTERS,starting_address=127,output_value=coordinate_roll)
            self.master.execute(1,cst.WRITE_MULTIPLE_REGISTERS,starting_address=129,output_value=feature_)

        except Exception as e:
            logger.error(e,exc_info=True)

    def calibrate_4Points(self):
        ### 坐标值 -- 图像坐标与机器人坐标一一对应 （9点亦如此）
        pixel_coords = np.array([[279, 349], [455, 121], [250, 137], [460, 328], [274, 342], [455, 47], [198, 90], [525, 363]])  
        robot_coords = np.array([[180.721, -100.987], [130.658, -51.02], [127.683, -102.998], [180.652, -53.98], [179.686,-101.957], [110.68,-47.042], [113.66,-114.962], [192.6618,-37.973]])  
        
        ### 构建增广矩阵 (A | B)  
        A = np.hstack((pixel_coords, np.ones((pixel_coords.shape[0], 1))))  
        B = robot_coords  
        
        ### 求解线性方程组  
        self.coefficients, _, _, _ = np.linalg.lstsq(A, B, rcond=None)  
        ### 因为lstsq返回的系数可能是列向量，我们需要转置它以匹配仿射变换的公式  
        self.coefficients = self.coefficients.T  

    ####此函数为了计算像素到物理位置的换算
    def count_position(self,value):
        # value：输入的是三个值
        try:
            robot_x = self.coefficients[0, 0] * value[0] + self.coefficients[0, 1] * value[1] + self.coefficients[0, 2]  
            robot_y = self.coefficients[1, 0] * value[0] + self.coefficients[1, 1] * value[1] + self.coefficients[1, 2]  
            angle = value[2]
            return [robot_x, robot_y, angle] 

        except Exception  as e:
            logger.error(e,exc_info=True)

    def wait_over(self,):
        try:
            if self.auto_flag:
                while self.master.execute(1,cst.READ_HOLDING_REGISTERS,starting_address=100,quantity_of_x=1)[0] <100:
                    time.sleep(0.5)
                if self.auto_flag:
                    self.action_flag +=1
                    time.sleep(1)
                    if self.program_flag == 2:
                        count = 0
                        while self.auto_flag:
                            time.sleep(1)
                            count +=1
                            if count == 12:
                                break
                else:
                    self.action_flag =0
            else:
                self.action_flag = 0
    
        except Exception as e:
            logger.error(e,exc_info=True)

    def action_run(self,num): 
        try:

            if self.position != None:
                if num == 31:
                    cmd = 31
                    posi = self.position["1"]
                    if posi !=[]:
                        posi = self.count_position(posi)
                        self.tcp_send(value=posi,cmd=cmd)
                        self.gui_signal.emit(json.dumps({"source":1,"name":"QR1"}))
                    else:
                        self.gui_signal.emit(json.dumps({"source":0,"name":"QR1"}))
                    self.wait_over()
                
                elif num == 32:
                    cmd = 32
                    posi = self.position["2"]
                    if posi !=[]:
                        posi = self.count_position(posi)
                        self.tcp_send(value=posi,cmd=cmd)
                        self.gui_signal.emit(json.dumps({"source":1,"name":"QR2"}))
                    else:
                        self.gui_signal.emit(json.dumps({"source":0,"name":"QR2"}))
                    self.wait_over()

                elif num == 33:
                    cmd = 33
                    posi = self.position["3"]
                    if posi !=[]:
                        posi = self.count_position(posi)
                        self.tcp_send(value=posi,cmd=cmd)
                        self.gui_signal.emit(json.dumps({"source":1,"name":"QR3"}))
                    else:
                        self.gui_signal.emit(json.dumps({"source":0,"name":"QR3"}))
                    self.wait_over()
                elif num == 34:
                    cmd = 34
                    posi = self.position["4"]
                    if posi !=[]:
                        posi = self.count_position(posi)
                        self.tcp_send(value=posi,cmd=cmd)
                        self.gui_signal.emit(json.dumps({"source":1,"name":"QR4"}))
                    else:
                        self.gui_signal.emit(json.dumps({"source":0,"name":"QR4"}))
                    self.wait_over()
                elif num == 41:
                    cmd = 41
                    posi = self.position["A"]
                    if posi !=[]:
                        posi = self.count_position(posi)
                        self.tcp_send(value=posi,cmd=cmd)
                        self.gui_signal.emit(json.dumps({"source":1,"name":"A"}))
                    else:
                        self.gui_signal.emit(json.dumps({"source":0,"name":"A"}))
                    self.wait_over()
                elif num == 42:
                    cmd = 42
                    posi = self.position["B"]
                    if posi !=[]:
                        posi = self.count_position(posi)
                        self.tcp_send(value=posi,cmd=cmd)
                        self.gui_signal.emit(json.dumps({"source":1,"name":"B"}))
                    else:
                        self.gui_signal.emit(json.dumps({"source":0,"name":"B"}))
                    self.wait_over()
                elif num == 43:
                    cmd = 43
                    posi = self.position["C"]
                    if posi !=[]:
                        posi = self.count_position(posi)
                        self.tcp_send(value=posi,cmd=cmd)
                        self.gui_signal.emit(json.dumps({"source":1,"name":"C"}))
                    else:
                        self.gui_signal.emit(json.dumps({"source":0,"name":"C"}))
                    self.wait_over()
                elif num == 44:
                    cmd = 44
                    posi = self.position["D"]
                    if posi !=[]:
                        posi = self.count_position(posi)
                        self.tcp_send(value=posi,cmd=cmd)
                        self.gui_signal.emit(json.dumps({"source":1,"name":"D"}))
                    else:
                        self.gui_signal.emit(json.dumps({"source":0,"name":"D"}))
                    self.wait_over()
                
                elif num == 11:
                    cmd = 11
                    posi = self.position["Red"]
                    if posi !=[]:
                        posi = self.count_position(posi)
                        self.tcp_send(value=posi,cmd=cmd)
                        self.gui_signal.emit(json.dumps({"source":1,"name":"Red"}))
                    else:
                        self.gui_signal.emit(json.dumps({"source":0,"name":"Red"}))
                elif num == 12:
                    cmd = 12
                    posi = self.position["Blue"]
                    if posi !=[]:
                        posi = self.count_position(posi)
                        self.tcp_send(value=posi,cmd=cmd)
                        self.gui_signal.emit(json.dumps({"source":1,"name":"Blue"}))
                    else:
                        self.gui_signal.emit(json.dumps({"source":0,"name":"Blue"}))
                elif num == 14:
                    cmd = 14
                    posi = self.position["Green"]
                    if posi !=[]:
                        posi = self.count_position(posi)
                        self.tcp_send(value=posi,cmd=cmd)
                        self.gui_signal.emit(json.dumps({"source":1,"name":"Green"}))
                    else:
                        self.gui_signal.emit(json.dumps({"source":0,"name":"Green"}))
                elif num == 15:
                    cmd = 15
                    posi = self.position["Yellow"]
                    if posi !=[]:
                        posi = self.count_position(posi)
                        self.tcp_send(value=posi,cmd=cmd)
                        self.gui_signal.emit(json.dumps({"source":1,"name":"Yellow"}))
                    else:
                        self.gui_signal.emit(json.dumps({"source":0,"name":"Yellow"}))
                
                elif num == 21:
                    cmd = 21
                    posi = self.position["circle"]
                    if posi !=[]:
                        posi = self.count_position(posi)
                        self.tcp_send(value=posi,cmd=cmd)
                        self.gui_signal.emit(json.dumps({"source":1,"name":"circle"}))
                    else:
                        self.gui_signal.emit(json.dumps({"source":0,"name":"circle"}))
                elif num == 22:
                    cmd = 22
                    posi = self.position["triangle"]
                    if posi !=[]:
                        posi = self.count_position(posi)
                        self.tcp_send(value=posi,cmd=cmd)
                
                        self.gui_signal.emit(json.dumps({"source":1,"name":"triangle"}))
                    else:
                        self.gui_signal.emit(json.dumps({"source":0,"name":"triangle"}))
                elif num == 23:
                    cmd = 23
                    posi = self.position["hexagon"]
                    if posi !=[]:
                        posi = self.count_position(posi)    
                        self.tcp_send(value=posi,cmd=cmd)
                        self.gui_signal.emit(json.dumps({"source":1,"name":"hexagon"}))
                    else:
                        self.gui_signal.emit(json.dumps({"source":0,"name":"hexagon"}))
                elif num == 24:
                    cmd = 24
                    posi = self.position["rectangle"]
                    if posi !=[]:
                        posi = self.count_position(posi)
                        self.tcp_send(value=posi,cmd=cmd)
                        self.gui_signal.emit(json.dumps({"source":1,"name":"rectangle"}))
                    else:
                        self.gui_signal.emit(json.dumps({"source":0,"name":"rectangle"}))


        except Exception as e:
            logger.error(e,exc_info=True)

    def run(self,):
        while self.run_flag:
            try:
                if self.run_flag:
                    time.sleep(0.1)
                    # print("action_flag is ", self.action_flag)
                    self.action_flag = self.master.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=182,quantity_of_x=1)[0]
                    if self.action_flag > 0:
                        self.action_run(self.action_flag)
                        self.action_flag = 0

            except Exception as e:
                logger.error(e,exc_info=True)




class IOACamera():

    def __init__(self, server_ip = "127.0.0.1", server_port=8527, head_ver=0x02, device_id = 1, camera_id = 1):
        try:
            self.socket_lock = threading.Lock()
            self.image_lock = threading.Lock()
            self.weather_lock = threading.Lock()
            #self.total_data = bytes()
            #self.recv_data = bytes()
            self.server_ip = server_ip
            self.server_port = server_port
            self.HEADER_VER = head_ver
            self.device_id = device_id
            self.camera_id = camera_id
            self.video_image = {}
            self.start_camera_data = {
                "cmd": "getImg",
                "deviceType": "\u89c6\u89c9\u76f8\u673a", 
                "deviceId": self.device_id,
                "cameraNum": self.camera_id
                }
            self.stop_camera_data = {
                "cmd": "stop"
            }

            ## 创建 socket 对象
            self.conn = socket.socket()         
            self.conn.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF, 5000)
            self.conn.connect((self.server_ip, self.server_port))
            # self.conn.setblocking(False)
            threading.Thread(target=self.recv_socket_data, args=()).start()
        except Exception as e:
            print(e)
            exit(0)
    
    def op_camera(self, data):
        send_data = json.dumps(data)
        #print(send_data)
        self.socket_lock.acquire()
        recv = self.pack_data_frame(send_data)
        self.send_socket_data(recv)
        self.socket_lock.release()
        return 0

    # send socket数据
    def send_socket_data(self, data):
        try:
            self.conn.send(data)
        except Exception as e:
            print(e)
    
    # 接受来自socket服务端的数据，并解析
    # socket服务端大约400ms返回一次数据
    def recv_socket_data(self):
        if 'debian' in platform.platform():
            while True:
                try:
                    total_data = bytes()
                    while True:
                        recv_data = self.conn.recv(1460)
                        total_data += recv_data
                        if(len(recv_data) != 1460):
                            break
                    parsed_data = self.unpack_data_frame(total_data)
                    if type(parsed_data) == type("a"):
                        parsed_data = json.loads(parsed_data)

                        if "humi" in parsed_data.keys():
                            self.weather_lock.acquire()
                            self.weather = parsed_data
                            self.weather_lock.release()
                        elif "cameraImg" in parsed_data.keys():
                            self.image_lock.acquire()
                            self.video_image = parsed_data
                            self.image_lock.release()
                    time.sleep(0.4)
                except Exception as e:
                    print(e)
        else:

            while True:
                try:                
                    #print("{} recv data".format(int(round(time.time() * 1000))))
                    recv_data = self.conn.recv(1 << 20)
                    parsed_data = self.unpack_data_frame(recv_data)
                    if type(parsed_data) == type("a"):
                        parsed_data = json.loads(parsed_data)

                        if "humi" in parsed_data.keys():
                            self.weather_lock.acquire()
                            self.weather = parsed_data
                            self.weather_lock.release()
                        elif "cameraImg" in parsed_data.keys():
                            self.image_lock.acquire()
                            self.video_image = parsed_data
                            self.image_lock.release()
                    time.sleep(0.4)
                except Exception as e:
                    print(e)

    # 打包data数据为bytes帧
    # data：json命令报文部分
    def pack_data_frame(self, data):
        cs = 0x00
        buf = bytes(data, encoding='utf-8')
        size = len(buf)+10
        head_tag = 0xaaff
        head_ver = self.HEADER_VER
        head_len = size-4
        tail_tag = 0x55ff
        buffer = b''
        tmp = struct.pack('<HBI', head_tag, head_ver, head_len)
        buffer += tmp
        buffer += buf
        cs = self.checksum(buffer, size)
        tmp = struct.pack('<BH', cs, tail_tag)
        buffer += tmp
        return buffer

    # 从buffer中解包
    # 异常返回0
    # 正常返回长度大于0的字符串
    def unpack_data_frame(self, buffer):
        length = len(buffer)
        size = 0
        if length < 10:
            print("length <10 return")
            return 0
        # <代表小端
        # H代表unsigned short
        # B代表unsigned char
        # I代表unsigner int
        # 格式化解析buffer[0-7]
        head_tag, head_ver, head_len = struct.unpack('<HBI', buffer[0:7])
        if (head_tag == 0xaaff):
            size = head_len+4
            if length < size:
                print("length {} < size {} return".format(length, size))
                return 0
            cs, tail_tag = struct.unpack('<BH', buffer[size-3:size])
            if ((tail_tag == 0x55ff) and (cs == self.checksum(buffer, size))):
                # print(size)
                data = str(buffer[7:size-3], encoding='utf-8')
                return data
            else:
                return 0
        else:
            return 0
    
    # 计算校验和
    # 校验和规则为：命令类型+帧长度+数据
    # buffer：[x,x,x,x]格式
    # size：有效数据的长度
    def checksum(self, buffer, size):
        cs = 0
        i = 2
        j = size-3
        while i < j:
            cs += buffer[i]
            i += 1
        return cs & 0xff

    # 关闭socket
    def close_socket(self):
        self.conn.close()

    def fetch_image(self):
        image = {}
        self.image_lock.acquire()
        image = self.video_image
        self.image_lock.release()
        return image

    def get_image(self):
        while True:
            try:
                self.op_camera(self.start_camera_data)
                img_b64decode = self.fetch_image()['cameraImg']
                img_data = base64.b64decode(img_b64decode)
                img_array = np.fromstring(img_data, np.uint8)
                img = cv2.imdecode(img_array, cv2.COLOR_BGR2RGB)
                self.frame = img
                break
            except Exception as e:
                pass
        return img

    def show(self):
        cv2.imshow('IOA', self.frame)
        cv2.waitKey(1)

    def close(self):
        cv2.destroyAllWindows()

